#ifndef EM80_DRIVER_H
#define EM80_DRIVER_H

#include <string>

#include <ros/ros.h>

// commands
#include <em80_msgs/BaseControl.h>
#include <em80_msgs/DockingConfig.h>
#include <em80_msgs/LEDConfig.h>
#include <em80_msgs/GPIOConfig.h>
#include <em80_msgs/ADCConfig.h>
#include <em80_msgs/DACConfig.h>
#include <em80_msgs/PowerSourceConfig.h>
#include <em80_msgs/JoystickMode.h>
#include <em80_msgs/FWVersionEnquiry.h>
#include <em80_msgs/DockingEnquiry.h>
#include <em80_msgs/StatusEnquiry.h>


// responses
#include <em80_msgs/CoreSensors.h>
#include <em80_msgs/Docking.h>
#include <em80_msgs/Battery.h>
#include <em80_msgs/GPInput.h>
#include <em80_msgs/ADC.h>
#include <em80_msgs/Firmware.h>
#include <em80_msgs/Status.h>

//ros subscribe
#include <geometry_msgs/Twist.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Range.h>
#include <sensor_msgs/Imu.h>



#include <boost/optional.hpp>   //limit

#include "em80_interface.h"
#include "packets.hpp"

namespace em80_driver
{

//robot paramete configuration
#define SONAR_SENSOR 1
#define SONAR_VIEW 1
#define SONAR_RANGE_MAX  2.99
#define SONAR_RANGE_MIN  0.03


class EM80Driver
{
public:
    EM80Driver(ros::NodeHandle nh, ros::NodeHandle private_nh);
    
    const float pi = 3.14159f;
    const float ratio = 1000.0f ;   //转速转换比例，执行速度调整比例
    const float D = 0.488f ;    //两轮间距，单位是m

    const float encoder_ppr = 25000;  //编码器线数
    const float wheel_radius = 0.07719; //轮子的半径

    const float var_len = (50.0f/encoder_ppr*2.0f*pi*wheel_radius)*(50.0f/encoder_ppr*2.0f*pi*wheel_radius);
    const float var_angle = (0.01f/180.0f*pi)*(0.01f/180.0f*pi);

    int32_t position_x_temp = 0, position_y_temp = 0;
	int16_t vel_linear_temp = 0, vel_angular_temp = 0;
	int16_t oriention_temp = 0;

	float position_x = 0, position_y = 0;
	float vel_linear = 0, vel_angular = 0;
	float oriention = 0;

    int16_t l_acc_x_temp = 0, l_acc_y_temp = 0, l_acc_z_temp = 0;
    float l_acc_x = 0, l_acc_y = 0, l_acc_z = 0;

    sensor_msgs::Range sonar0, sonar1, sonar2, sonar3, sonar4, sonar5, sonar6, sonar7;
	uint16_t sonar0_tmp, sonar1_tmp, sonar2_tmp, sonar3_tmp, sonar4_tmp, sonar5_tmp, sonar6_tmp, sonar7_tmp;
	float sonar0_range = 0, sonar1_range = 0, sonar2_range = 0, sonar3_range = 0, sonar4_range = 0, sonar5_range = 0, sonar6_range = 0, sonar7_range = 0;

    sensor_msgs::Imu imu_data;
private:
    // interface to the EM80
    EM80Interface em80_;
    void em80PayloadCallback(const boost::shared_ptr<EM80PayloadBase const> &payload);
    void em80ErrorCallback(const std::string &error);

    // limits

    // ROS services
    ros::Subscriber velocity_sub;
    ros::Subscriber docking_config_sub;
    ros::Subscriber led_config_sub;
    ros::Subscriber gpio_config_sub;
    ros::Subscriber adc_config_sub;
    ros::Subscriber dac_config_sub;
    ros::Subscriber power_source_config_sub;
    ros::Subscriber joystick_mode_sub;
    ros::Subscriber firmware_enquiry_sub;
    ros::Subscriber docking_enquiry_sub;
    ros::Subscriber status_enquiry_sub;


    ros::Publisher  odom_pub;
    ros::Publisher  imu_pub;
    ros::Publisher  ultrasonic_pub_1;
    ros::Publisher  ultrasonic_pub_2;
    ros::Publisher  ultrasonic_pub_3;
    ros::Publisher  ultrasonic_pub_4;
    ros::Publisher  ultrasonic_pub_5;
    ros::Publisher  ultrasonic_pub_6;
    ros::Publisher  ultrasonic_pub_7;
    ros::Publisher  ultrasonic_pub_8;


    // ros::Publisher  core_sensors_pub;
    ros::Publisher  docking_pub;
    ros::Publisher  battery_pub;
    ros::Publisher  gp_input_pub;
    ros::Publisher  adc_pub;
    ros::Publisher  firmware_pub;
    ros::Publisher  status_pub;

    //ros::Timer  timer;

    // ROS callbacks
    void velocityCallback(const geometry_msgs::Twist &velocity_config);
    void dockingConfigCallback(const em80_msgs::DockingConfigConstPtr &docking_config);
    void ledConfigCallback(const em80_msgs::LEDConfigConstPtr &led_config);
    void gpioConfigCallback(const em80_msgs::GPIOConfigConstPtr &gpio_config);
    void adcConfigCallback(const em80_msgs::ADCConfigConstPtr &adc_config);
    void dacConfigCallback(const em80_msgs::DACConfigConstPtr &dac_config);
    void powerSourceConfigCallback(const em80_msgs::PowerSourceConfigConstPtr &power_source_config);
    void joystickModeCallback(const em80_msgs::JoystickModeConstPtr &joystick_mode);
    void firmwareEnquiryCallback(const em80_msgs::FWVersionEnquiryConstPtr &firmware_enquiry);
    void dockingEnquiryCallback(const em80_msgs::DockingEnquiryConstPtr &docking_enquiry);
    void statusEnquiryCallback(const em80_msgs::StatusEnquiryConstPtr &status_enquiry);
};

}

#endif
